#!/usr/bin/python3
"""
Show a 3d mesh with vispy.
usage:
python3 Viewer.py [-i] -f path_to_mesh_file

By zen shawn Aug 16th, 2020
"""

import argparse
import sys
from threading import Thread
import trimesh

from IPython import embed
import numpy as np
import vispy
import vispy.visuals
from vispy import app, scene
from vispy.scene import transforms
from modules.MeshLoader import *
from modules.RobotController import Robot
from modules.Utils import *

logger = logging.getLogger(__file__)
np.set_printoptions(precision=4, suppress=True)
# argument parser
parser = argparse.ArgumentParser(
    description="Plot mesh or point cloud in vispy")
parser.add_argument("-m", "--mesh-file", type=str, help="input mesh file")
parser.add_argument("-f", "--show-mesh-faces",
                    action="store_false", default=True, help="not show mesh triangle faces")
parser.add_argument("-e", "--show-mesh-edges",
                    action="store_true", default=False, help="show mesh triangle edges")
parser.add_argument("-p", "--point-cloud", type=str,
                    help="input point cloud txt file, could be parsed to Nx3 ndarray with np.loadtxt")
parser.add_argument("-u", "--urdf-file", type=str,
                    help="input robot urdf file, if success, variable `robot` is available then")
parser.add_argument("-i", "--interactive",
                    action="store_true", default=False, help="enter interactive mode, please use `python3 -i Viewer.py` instead")
ns = parser.parse_args()

# vispy setup
vispy.use(app="pyside2")
canvas = scene.SceneCanvas(
    title="MeshDisplay", size=(800, 600), bgcolor="white", keys="interactive")
view = canvas.central_widget.add_view()
view.camera = scene.cameras.TurntableCamera()
view.add(scene.visuals.XYZAxis())
canvas.unfreeze()
canvas.view = view
canvas.freeze()
canvas.show()
timer = vispy.app.Timer()
timer.start()

robot = None


@canvas.connect
def on_key_press(event):
    if event.key.name == vispy.keys.SPACE and robot is not None:
        robot.RandomState(False)
        logger.info("following trajectory")


if ns.mesh_file:
    LoadMesh(ns.mesh_file, view, show_face=ns.show_mesh_faces,
             show_edge=ns.show_mesh_edges)

if ns.urdf_file:
    robot = Robot()
    robot.Show(ns.urdf_file, view.canvas)

if ns.point_cloud:
    pos = np.loadtxt(ns.point_cloud)
    if pos.shape[1] == 3:
        markers = visuals.Markers()
        markers.set_data(pos, edge_color="grey",
                         face_color=(1, 0.1, 1, .5), size=3)
        view.add(markers)
    else:
        logger.error("point cloud should be Nx3 array")


if ns.interactive:
    @timer.connect
    def on_timer(event):
        canvas.update()
    vispy.app.set_interactive()
    embed()
else:
    # vispy loop
    canvas.app.run()
